THRESHOLD = (24, 60, 2, 62, 3, 60) # Grayscale threshold for dark things...
#THRESHOLD = (26, 61, 13, 69, -27, 15)
import sensor, image, time
from image import SEARCH_EX, SEARCH_DS
#import car
from pid import PID
from struct import *
from pyb import UART
import lcd

uart = UART(3, 115200, timeout_char=1000)                         # i使用给定波特率初始化
uart.init(115200, bits=8, parity=None, stop=1, timeout_char=1000) # 使用给定参数初始化


rho_pid = PID(p=0.4, i=0)
theta_pid = PID(p=0.001, i=0)

sensor.reset()
sensor.set_contrast(1)
sensor.set_gainceiling(16)
#sensor.set_vflip(True)
#sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
#sensor.set_windowing([0,20,80,40])
sensor.skip_frames(time = 2000)     # WARNING: If you use QQVGA it may take seconds

## Load template.
## Template should be a small (eg. 32x32 pixels) grayscale image.
cross_roads = []
t_roads = []
for i in range(1, 4):
    cross_roads.append(image.Image("/cross_road" + str(i) + ".pgm"))
    t_roads.append(image.Image("/t_road" + str(i) + ".pgm"))
##加载模板图片
lcd.init()
clock = time.clock()                # to process a frame sometimes.
output = 0

while(True):
    clock.tick()
    img = sensor.snapshot()#.binary([THRESHOLD])
    
    line = img.get_regression([THRESHOLD], robust = True)
    if (line):
        rho_err = abs(line.rho())-img.width()/2
        if line.theta()>90:
            theta_err = line.theta()-180
        else:
            theta_err = line.theta()
        img.draw_line(line.line(), color = 127)
        # print(rho_err,line.magnitude(),rho_err)
        if line.magnitude()>8:
            #if -40<b_err<40 and -30<t_err<30:
            rho_output = rho_pid.get_pid(rho_err,1)
            theta_output = theta_pid.get_pid(theta_err,1)
            output = rho_output+theta_output
            #car.run(50+output, 50-output)
        #else:
            #car.run(0,0)
    else:
        #car.run(50,-50)
        pass

    buf = pack("<BBIB", 0x0a, 0x00, int(output*10), 0xa0)
    img.to_grayscale()
    #find_template(template, threshold, [roi, step, search]),threshold中
    #的0.7是相似度阈值,roi是进行匹配的区域（左上顶点为（10，0），长80宽60的矩形），
    #注意roi的大小要比模板图片大，比frambuffer小。
    #把匹配到的图像标记出来
    for i in range(1, 4):
        cross = img.find_template(cross_roads[i - 1], 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
        t = img.find_template(t_roads[i - 1], 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
        if cross:
            img.draw_rectangle(cross)
            buf = pack("<BBIB", 0x0a, 0x01, 0, 0xa0)
        elif t:
            img.draw_rectangle(t)
            buf = pack("<BBIB", 0x0a, 0x01, 0, 0xa0)

    print(buf)
    uart.write(buf)
    lcd.display(img)
    #print(clock.fps())
